
#include <stdio.h>
#include <math.h>


#define NANOSEC 1000000000
#define MICROSEC 1000000
#define MILLISEC 1000
#define FOSC 20000000
#define TPNS (4*(NANOSEC / FOSC))

#define STEPS360 200

//unsigned short ta;
int tint;
short tsint;
unsigned int treg;
unsigned int treginitial;
unsigned int tregfinal;
unsigned short vinitial, vfinal;
signed int totalreg;
signed int constreg;
unsigned int step;
unsigned char phase;

double t0, t1, t, sigmat, tr,alpha, delta, b, s, v;
double ta;
/*
unsigned short compute_timer(unsigned short v) {
  unsigned int tc = (360 * MICROSEC) / STEPS360;
  return (unsigned short)((tc * 1000) / (v * TPNS));
}
*/
unsigned int compute_timer(unsigned int v) {
  unsigned int tc = (360 * MICROSEC) / STEPS360;
  return (unsigned int)((tc * 1000) / (v * TPNS));
}

int main(void) {
  vinitial=1;
  vfinal = 360;
  alpha = 180;
  phase = (vfinal >= vinitial ? 0 : 1);

  tregfinal = compute_timer(vfinal);
  printf("%d\n", tregfinal);
  
  b = (double)vinitial * STEPS360;


  if (phase == 0) {
    delta = (b*b) + 4 * alpha * 360 * STEPS360;
    t0 = -b + sqrt(delta);
  } else {
    delta = (b*b) - 4 * alpha * 360 * STEPS360;
    t0 = b - sqrt(delta);
  }
  t0 = t0 / (2 * alpha * STEPS360);
  //
  treg = (int) (t0 * (NANOSEC / TPNS));
  tr = (t0 * (NANOSEC / TPNS));
  t = t0;
  step = 0;
  v=360 / (STEPS360 * t);
  ta=t0;
  tint = (int) (ta  * NANOSEC);
  //t0 = 360 / (STEPS360 * (double)vinitial);
  //sigmat+=t0;
  //printf("%0.9g %g %g %g %g %g %d\n", 0.0, b, b*b, delta, sqrt(delta), sqrt(delta) - b, step);
  //printf("%0.9g %g %g %g %g %g %g %g %d %g %u\n", 0.0, b, b*b, delta, sqrt(delta), sqrt(delta) - b, t, ta, step, tr, treg);
    printf("%0.9g %d %g %g %g %u\n", 0.0, step, t, tr, ta, treg);
  //printf("%0.9g %g %u %u %g\n", 0.0, tr, step, compute_timer((unsigned int)tr), v);
  /* t1=360 / (STEPS360 * ((360 / (STEPS360 * t0)) + alpha * t0));
  tr = t1 * (NANOSEC / TPNS);
  t = t1;
  step += 1;
  printf("%0.9g %g %u %u\n", t0, tr, step, compute_timer((unsigned int)tr));
  sigmat+=t;*/
  sigmat=t;
  while(1) {
    double lasttr=tr, lalpha, lbeta;
    unsigned int laststep = step;
    step += 1;
    /*s = (double) (step + 1);
    b = (sigmat + t0 * (1 - 2*s));
    delta = b * b - 4 * sigmat * t0;
    //printf("b=%g delta2=%g\n", b, delta);
    t = -(sigmat + t0 * (1 - 2*s)) - sqrt(delta);
    t = t / 2;
    */
    b = v * STEPS360;
  
    //printf("b=%g delta2=%g\n", b, delta);
    if (phase ==0) {
      delta = (b*b) + 4 * alpha * 360 * STEPS360;
      t = -b + sqrt(delta);
    } else {
      delta = (b*b) - 4 * alpha * 360 * STEPS360;
      t = b - sqrt(delta);
    }
    t = t / (2 * alpha * STEPS360);
  
    tr = (int) (t * (NANOSEC / TPNS));
    v=360 / (STEPS360 * t);
    if (phase ==0) {
      int tcomp;
      ta = ta - (alpha * STEPS360 / 360) * (ta*ta*ta);
      tsint = (short)(tint >> 15);
      tcomp = tsint * tsint;
      if (tcomp > (1 << 15)) tcomp = (tcomp >> 15);
      tcomp = tcomp*tsint;
      tint = tint - (alpha * STEPS360 * tcomp) / 360;
    } else {
      int tcomp;
      ta = ta + (alpha * STEPS360 / 360) * (ta*ta*ta);
      tsint = (short)(tint >> 15);
      tcomp = tsint * tsint;
      if (tcomp > (1 << 15)) tcomp = (tcomp >> 15);
      tcomp = tcomp*tsint;
      tint = tint + (alpha * STEPS360 * tcomp) / 360;
    }
    //t = sigmat / ((2 * (double)step) - (sigmat / t0)); 
    treg = (int) (ta * (NANOSEC / TPNS));
    //printf("%0.9g %g %g %g %g %g %d \n", sigmat, b, b*b, delta, sqrt(delta), sqrt(delta) - b, step);
    //printf("%0.9g %g %g %g %g %g %g %g %d %g %u\n", sigmat, b, b*b, delta, sqrt(delta), sqrt(delta) - b, t, ta, step, tr, treg);
    printf("%0.9g %d %g %g %g %u\n", sigmat, step, t, tr, ta, treg);
    //printf("%.09g %g %u %u %g\n", sigmat, tr, step, compute_timer((unsigned int)tr), v);
    //printf("%.09g %g %g %d %g\n", sigmat, log2(tr), log2(step), step, tr);
    if (step > 1) {

      lalpha = (log2(tr) - log2(lasttr))/(log2(step) - log2(laststep));
      lbeta = 0.5 * ((log2(tr) + log2(lasttr)) - lalpha * (log2(step) - log2(laststep)));
      //printf("%.09g %g %g\n", sigmat, lalpha, lbeta);
    }

    sigmat += t;
    //treg = (unsigned int) tr;
    if ((phase == 0) && (treg <= tregfinal)) break;
    if ((phase == 1) && (treg >= tregfinal)) break;
  }
}
